//
// Created by engineai on 2024/07/03.
//
/*!
 * @file ContactSpringDamper.cpp
 * @brief Spring Damper based Contact dynamics
 */

#include "Collision/ContactSpringDamper.h"
#include "Dynamics/FloatingBaseModel.h"

/*!
 * compute contact forces coinciding to the contact and update external force
 * in a dynamics model.
 */
template <typename T>
void ContactSpringDamper<T>::UpdateExternalForces(T K, T D, T dt)
{
    CC::_nContact = CC::_CheckContact();
    for (size_t i(0); i < _nGC; ++i)
    {
        // first assume there's no contact, so the ground "springs back"
        deflectionRate[i] = (-K / D) * _tangentialDeflections[i];
    }
    if (CC::_nContact > 0)
    {
        _groundContactWithOffset(K, D);
        for (size_t i(0); i < CC::_nContact; ++i)
        {
            CC::_model->_externalForces.at(CC::_model->_gcParent[CC::_idx_list[i]]) +=
                forceToSpatialForce(CC::_cp_force_list[CC::_idx_list[i]],
                                    CC::_cp_pos_list[i]);
        }
        static int count(0);
        ++count;
        if (count > 5)
        {
            // exit(0);
        }
    }

    for (size_t i(0); i < _nGC; ++i)
    {
        _tangentialDeflections[i] += dt * deflectionRate[i];
    }
}

/*!
 * subfunction computing contact forces
 */
template <typename T>
void ContactSpringDamper<T>::_groundContactWithOffset(T K, T D)
{
    for (size_t i = 0; i < CC::_nContact; i++)
    {
        Vec3<T> v =
            CC::_cp_frame_list[i].transpose() *
            CC::_model->_vGC[CC::_idx_list[i]]; // velocity in plane coordinates
        T z = CC::_cp_penetration_list[i];      // the penetration into the ground
        T zd = v[2];                            // the penetration velocity
        T zr = std::sqrt(std::max(T(0), -z));   // sqrt penetration into the ground,
        // or zero if we aren't penetrating
        T normalForce =
            zr *
            (-K * z - D * zd); // normal force is spring-damper * sqrt(penetration)

        // set output force to zero for now.
        CC::_cp_local_force_list[i][0] = 0;
        CC::_cp_local_force_list[i][1] = 0;
        CC::_cp_local_force_list[i][2] = 0;

        if (normalForce > 0)
        {
            CC::_cp_local_force_list[i][2] =
                normalForce; // set the normal force. This is in the plane's
            // coordinates for now

            // first, assume sticking
            // this means the tangential deformation happens at the speed of the foot.
            deflectionRate[CC::_idx_list[i]] = v.template topLeftCorner<2, 1>();
            Vec2<T> tangentialSpringForce =
                K * zr *
                _tangentialDeflections[CC::_idx_list[i]]; // tangential force due to
            // "spring"
            Vec2<T> tangentialForce =
                -tangentialSpringForce -
                D * zr * deflectionRate[CC::_idx_list[i]]; // add damping to get
            // total tangential

            // check for slipping:
            T slipForce = CC::_cp_mu_list[i] *
                          normalForce; // maximum force magnitude without slipping
            T tangentialForceMagnitude =
                tangentialForce
                    .norm();                            // actual force magnitude if we assume sticking
            T r = tangentialForceMagnitude / slipForce; // ratio of force/max_force

            if (r > 1)
            {
                // we are slipping.
                tangentialForce =
                    tangentialForce / r; // adjust tangential force to avoid slipping
                deflectionRate[CC::_idx_list[i]] =
                    -(tangentialForce + tangentialSpringForce) / (D * zr);
            }
            // set forces
            CC::_cp_local_force_list[i][0] = tangentialForce[0];
            CC::_cp_local_force_list[i][1] = tangentialForce[1];
        }
        // move back into robot frame
        CC::_cp_force_list[CC::_idx_list[i]] =
            CC::_cp_frame_list[i] * CC::_cp_local_force_list[i];
    }
}

template class ContactSpringDamper<double>;
template class ContactSpringDamper<float>;
